"""
MAVLink 协议解析
由于部分 mavlink 实现没有在 pymavlink 中找到，或者是不能很好的引用，
这里实现了 plane telemetry 的所有 mavlink 消息解析后端。
这是一个完全的函数库 :)

date: 2025/7/15
author: SiHeng Tang
file: mav_messages.py
copyright(c) DFSA Eastern Software
This file contain code copy from pymavlink library using GPL License
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging

import numpy as np

# from pymavlink.dialects.v20 import ardupilotmega

logger = logging.getLogger(__name__)

# TODO 将这里的常量使用 ardupilot mega 包中的通用常量替代
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 0b00000001
MAV_MODE_FLAG_TEST_ENABLED = 0b00000010
MAV_MODE_FLAG_AUTO_ENABLED = 0b00000100
MAV_MODE_FLAG_GUIDED_ENABLED = 0b00001000
MAV_MODE_FLAG_STABILIZE_ENABLED = 0b00010000
MAV_MODE_FLAG_HIL_ENABLED = 0b00100000
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 0b01000000
MAV_MODE_FLAG_SAFETY_ARMED = 0b10000000

# 飞行模式，垂直起降变体和热气流模式不支持
AP_PLANE_MODE_NOT_SUPPORT = -1
AP_PLANE_MODE_MANUAL = 0
AP_PLANE_MODE_CIRCLE = 1
AP_PLANE_MODE_STABILIZE = 2
AP_PLANE_MODE_TRAINING = 3
AP_PLANE_MODE_ACRO = 4
AP_PLANE_MODE_FLY_BY_WIRE_A = 5
AP_PLANE_MODE_FLY_BY_WIRE_B = 6
AP_PLANE_MODE_CRUISE = 7
AP_PLANE_MODE_AUTOTUNE = 8
AP_PLANE_MODE_AUTO = 10
AP_PLANE_MODE_RTL = 11
AP_PLANE_MODE_LOITER = 12
AP_PLANE_MODE_TAKEOFF = 13
AP_PLANE_MODE_LAND = 14
AP_PLANE_MODE_GUIDED = 15
AP_PLANE_MODE_INITIALIZING = 16

# 飞行模式字符串，用于用户界面显示和日志
PLANE_FLIGHT_MODES_STRING = {
    AP_PLANE_MODE_NOT_SUPPORT: "NOT_SUPPORT",
    AP_PLANE_MODE_MANUAL: "MANUAL",
    AP_PLANE_MODE_CIRCLE: "CIRCLE",
    AP_PLANE_MODE_STABILIZE: "STABILIZE",
    AP_PLANE_MODE_TRAINING: "TRAINING",
    AP_PLANE_MODE_ACRO: "ACRO",
    AP_PLANE_MODE_FLY_BY_WIRE_A: "FLY_BY_WIRE_A",
    AP_PLANE_MODE_FLY_BY_WIRE_B: "FLY_BY_WIRE_B",
    AP_PLANE_MODE_CRUISE: "CRUISE",
    AP_PLANE_MODE_AUTOTUNE: "AUTOTUNE",
    AP_PLANE_MODE_AUTO: "AUTO",
    AP_PLANE_MODE_RTL: "RTL",
    AP_PLANE_MODE_LOITER: "LOITER",
    AP_PLANE_MODE_TAKEOFF: "TAKEOFF",
    AP_PLANE_MODE_LAND: "LAND",
    AP_PLANE_MODE_GUIDED: "GUIDED",
    AP_PLANE_MODE_INITIALIZING: "INITIALIZING",
}

# GPS 定位类型
GPS_FIX_TYPE_NO_GPS = 0
GPS_FIX_TYPE_NO_FIX = 1
GPS_FIX_TYPE_2D_FIX = 2
GPS_FIX_TYPE_3D_FIX = 3
GPS_FIX_TYPE_DGPS = 4
GPS_FIX_TYPE_RTK_FLOAT = 5
GPS_FIX_TYPE_RTK_FIXED = 6
GPS_FIX_TYPE_STATIC = 7
GPS_FIX_TYPE_PPP = 8

# GPS 定位类型字符串
GPS_FIX_TYPE_STRING = {
    GPS_FIX_TYPE_NO_GPS: "NO_GPS_CONNECTED",
    GPS_FIX_TYPE_NO_FIX: "NO_POSITION_INFORMATION",
    GPS_FIX_TYPE_2D_FIX: "2D_POSITION_FIX",
    GPS_FIX_TYPE_3D_FIX: "3D_POSITION_FIX",
    GPS_FIX_TYPE_DGPS: "DGPS_SBAS_AIDED_3D_POSITION",
    GPS_FIX_TYPE_RTK_FLOAT: "RTK_FLOAT_POSITION",
    GPS_FIX_TYPE_RTK_FIXED: "RTK_FIXED_POSITION",
    GPS_FIX_TYPE_STATIC: "BASE_STATION_STATIC_FIX_POSITION",
    GPS_FIX_TYPE_PPP: "PPP_3D_POSITION"
}


################################################
# mavlink 消息特殊位解析
################################################
def __parse_mav_base_mode_bits(b: int):
    return {
        "MAV_MODE_FLAG_CUSTOM_MODE_ENABLED": bool(b & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
        "MAV_MODE_FLAG_TEST_ENABLED": bool(b & MAV_MODE_FLAG_TEST_ENABLED),
        "MAV_MODE_FLAG_AUTO_ENABLED": bool(b & MAV_MODE_FLAG_AUTO_ENABLED),
        "MAV_MODE_FLAG_GUIDED_ENABLED": bool(b & MAV_MODE_FLAG_GUIDED_ENABLED),
        "MAV_MODE_FLAG_STABILIZE_ENABLED": bool(b & MAV_MODE_FLAG_STABILIZE_ENABLED),
        "MAV_MODE_FLAG_HIL_ENABLED": bool(b & MAV_MODE_FLAG_HIL_ENABLED),
        "MAV_MODE_FLAG_MANUAL_INPUT_ENABLED": bool(b & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
        "MAV_MODE_FLAG_SAFETY_ARMED": bool(b & MAV_MODE_FLAG_SAFETY_ARMED)
    }


def __parse_mav_mission_current_state_bit(b: int):
    return {
        "MISSION_STATE_UNKNOWN": b == 0,
        "MISSION_STATE_NO_MISSION": b == 1,
        "MISSION_STATE_NOT_STARTED": b == 2,
        "MISSION_STATE_ACTIVE": b == 3,
        "MISSION_STATE_PAUSED": b == 4,
        "MISSION_STATE_COMPLETE": b == 5
    }


def __parse_mav_mission_current_mode_bit(b: int):
    return {
        "MISSION_MODE_UNKNOWN": b == 0,
        "MISSION_MODE_ON_MISSION": b == 1,
        "MISSION_MODE_OFF_MISSION": b == 2
    }


def __parse_mav_mission_ack_type_bit(b: int):
    return {
        "MAV_MISSION_TYPE_MISSION": b == 0,
        "MAV_MISSION_TYPE_FENCE": b == 1,
        "MAV_MISSION_TYPE_RALLY": b == 2,
        "MAV_MISSION_TYPE_ALL": b == 255
    }


def __parse_mav_mission_ack_result_bit(b: int):
    return {
        "MAV_MISSION_ACCEPTED": b == 0,
        "MAV_MISSION_ERROR": b == 1,
        "MAV_MISSION_UNSUPPORTED_FRAME": b == 2,
        "MAV_MISSION_UNSUPPORTED": b == 3,
        "MAV_MISSION_NO_SPACE": b == 4,
        "MAV_MISSION_INVALID": b == 5,
        "MAV_MISSION_INVALID_PARAM1": b == 6,
        "MAV_MISSION_INVALID_PARAM2": b == 7,
        "MAV_MISSION_INVALID_PARAM3": b == 8,
        "MAV_MISSION_INVALID_PARAM4": b == 9,
        "MAV_MISSION_INVALID_PARAM5_X": b == 10,
        "MAV_MISSION_INVALID_PARAM6_Y": b == 11,
        "MAV_MISSION_INVALID_PARAM7": b == 12,
        "MAV_MISSION_INVALID_SEQUENCE": b == 13,
        "MAV_MISSION_DENIED": b == 14,
        "MAV_MISSION_OPERATION_CANCELLED": b == 15
    }


def __parse_mav_gps_fix_type_bit(b: int):
    return {
        "GPS_FIX_TYPE_NO_GPS": b == GPS_FIX_TYPE_NO_GPS,
        "GPS_FIX_TYPE_NO_FIX": b == GPS_FIX_TYPE_NO_FIX,
        "GPS_FIX_TYPE_2D_FIX": b == GPS_FIX_TYPE_2D_FIX,
        "GPS_FIX_TYPE_3D_FIX": b == GPS_FIX_TYPE_3D_FIX,
        "GPS_FIX_TYPE_DGPS": b == GPS_FIX_TYPE_DGPS,
        "GPS_FIX_TYPE_RTK_FLOAT": b == GPS_FIX_TYPE_RTK_FLOAT,
        "GPS_FIX_TYPE_RTK_FIXED": b == GPS_FIX_TYPE_RTK_FIXED,
        "GPS_FIX_TYPE_STATIC": b == GPS_FIX_TYPE_STATIC,
        "GPS_FIX_TYPE_PPP": b == GPS_FIX_TYPE_PPP
    }


################################################
# mavlink 消息到 Python 字典
################################################
# noinspection SpellCheckingInspection
def parse_rc_channels(msg):
    logger.debug(f"parse RC_CHANNELS message"
                 f"{msg.time_boot_ms=} {msg.chancount=} {msg.chan1_raw=} {msg.chan2_raw=}"
                 f"{msg.chan3_raw=} {msg.chan4_raw=} {msg.chan5_raw=} {msg.chan6_raw=} {msg.chan7_raw=}"
                 f"{msg.chan8_raw=} {msg.chan9_raw=} {msg.chan10_raw=} {msg.chan11_raw=} {msg.chan12_raw=}"
                 f"{msg.chan13_raw=} {msg.chan14_raw=} {msg.chan15_raw=} {msg.chan16_raw=} {msg.chan17_raw=}"
                 f"{msg.chan18_raw=} {msg.rssi=}")

    return {
        "time_boot_ms": int(msg.time_boot_ms),
        "chancount": int(msg.chancount),
        "chan1_raw": int(msg.chan1_raw),
        "chan2_raw": int(msg.chan2_raw),
        "chan3_raw": int(msg.chan3_raw),
        "chan4_raw": int(msg.chan4_raw),
        "chan5_raw": int(msg.chan5_raw),
        "chan6_raw": int(msg.chan6_raw),
        "chan7_raw": int(msg.chan7_raw),
        "chan8_raw": int(msg.chan8_raw),
        "chan9_raw": int(msg.chan9_raw),
        "chan10_raw": int(msg.chan10_raw),
        "chan11_raw": int(msg.chan11_raw),
        "chan12_raw": int(msg.chan12_raw),
        "chan13_raw": int(msg.chan13_raw),
        "chan14_raw": int(msg.chan14_raw),
        "chan15_raw": int(msg.chan15_raw),
        "chan16_raw": int(msg.chan16_raw),
        "chan17_raw": int(msg.chan17_raw),
        "chan18_raw": int(msg.chan18_raw),
        "rssi": int(msg.rssi)
    }


def parse_mav_heartbeat(msg):
    logger.debug(f"parse HEARTBEAT message "
                 f"{msg.type=} {msg.autopilot=} {msg.base_mode=} {msg.custom_mode=} "
                 f"{msg.system_status=} {msg.mavlink_version=}")

    d = {
        "base_mode": msg.base_mode,
        "custom_mode": msg.custom_mode
    }
    d.update(__parse_mav_base_mode_bits(msg.base_mode))
    return d


def parse_mav_global_position(msg):
    logger.debug(f"parse GLOBAL_POSITION_INT message "
                 f"{msg.lat=} {msg.lon=} {msg.alt=} {msg.relative_alt=} {msg.vx=} {msg.vy=} {msg.vz=} {msg.hdg=}")

    return {
        "lat": msg.lat / 1e7,
        "lon": msg.lon / 1e7,
        "alt": msg.alt / 1e3,
        "relative_alt": msg.relative_alt / 1e3,
        "vx": msg.vx / 1e2,
        "vy": msg.vy / 1e2,
        "vz": msg.vz / 1e2,
        "hdg": msg.hdg / 1e2
    }


def parse_mav_attitude(msg):
    logger.debug(f"parse ATTITUDE message "
                 f"{msg.roll=} {msg.yaw=} {msg.pitch=}")

    return {
        "roll": np.degrees(msg.roll),
        "pitch": np.degrees(msg.pitch),
        "yaw": np.degrees(msg.yaw)
    }


def parse_mav_home_position(msg):
    logger.debug(f"parse HOME_POSITION message "
                 f"{msg.latitude=} {msg.longitude=} {msg.altitude=} "
                 f"{msg.x=} {msg.y=} {msg.z=} "
                 f"{msg.approach_x=} {msg.approach_y=} {msg.approach_z=}")

    return {
        "latitude": msg.latitude / 1e7,
        "longitude": msg.longitude / 1e7,
        "altitude": msg.altitude / 1e3,
        "x": msg.x,
        "y": msg.y,
        "z": msg.z,
        "approach_x": msg.approach_x,
        "approach_y": msg.approach_y,
        "approach_z": msg.approach_z
    }


def parse_mav_mission_current(msg):
    logger.debug(f"parse MISSION_CURRENT message "
                 f"{msg.seq=} {msg.total=} {msg.mission_state=} "
                 f"{msg.mission_mode=} msg.mission_id= msg.fence_id= "
                 f"msg.rally_points_id=")

    # FIXME 这里可能是 pymavlink 也可能是实现问题，mission_current 缺后三个 ID
    d = {
        "seq": msg.seq,
        "total": msg.total,
        "mission_status": msg.mission_state,
        "mission_mode": msg.mission_mode,
        # "mission_id": msg.mission_id,
        # "fence_id": msg.fence_id,
        # "rally_points_id": msg.rally_points_id
    }
    d.update(__parse_mav_mission_current_state_bit(msg.mission_state))
    d.update(__parse_mav_mission_current_state_bit(msg.mission_mode))
    return d


def parse_mav_mission_ack(msg):
    logger.debug(f"parse MISSION_ACK message "
                 f"{msg.type=} {msg.mission_type=} {msg.opaque_id=}")

    d = {
        "type": msg.type,
        "mission_type": msg.mission_type,
        "opaque_id": msg.opaque_id
    }
    d.update(__parse_mav_mission_ack_result_bit(msg.type))
    d.update(__parse_mav_mission_ack_type_bit(msg.mission_type))
    return d


def parse_mav_gps_raw_int(msg):
    logger.debug(f"parse GPS_RAW_INT message "
                 f"{msg.time_usec=} {msg.fix_type=} {msg.lat=} {msg.lon=} "
                 f"{msg.alt=} {msg.eph=} {msg.epv=} {msg.vel=} {msg.cog=} "
                 f"{msg.satellites_visible=} {msg.alt_ellipsoid=} "
                 f"{msg.h_acc=} {msg.v_acc=} {msg.vel_acc=} {msg.hdg_acc=} {msg.yaw=}")

    d = {
        "time_usec": int(msg.time_usec),
        "fix_type": int(msg.fix_type),
        "lat": float(msg.lat) / 1e7,
        "lon": float(msg.lon) / 1e7,
        "alt": msg.alt,
        "eph": float(msg.eph) / 1e2,
        "epv": float(msg.epv) / 1e2,
        "vel": msg.vel,
        "cog": msg.cog,
        "satellites_visible": msg.satellites_visible,
        "alt_ellipsoid": msg.alt_ellipsoid,
        "h_acc": msg.h_acc,
        "v_acc": msg.v_acc,
        "vel_acc": msg.vel_acc,
        "hdg_acc": float(msg.hdg_acc) / 1e5,
        "yaw": msg.yaw
    }

    d.update(__parse_mav_gps_fix_type_bit(msg.fix_type))

    return d


def parse_mav_system_time(msg):
    logger.debug(f"parse SYSTEM_TIME message "
                 f"{msg.time_unix_usec=} {msg.time_boot_ms=}")

    return {
        "time_unix_usec": msg.time_unix_usec / 1e6,
        "time_boot_ms": msg.time_boot_ms
    }
